% 初始化参数
A = eye(2); % 系统动态矩阵，这里假设状态不随时间变化
H = [1 0]; % 观测矩阵，这里假设观测值直接是状态的第一个元素
Q = 0.01 * eye(2); % 过程噪声协方差矩阵
R = 0.1; % 观测噪声协方差矩阵
x_est = [0; 0]; % 初始状态估计，这里假设初始状态是0
P_est = 1000 * eye(2); % 初始误差协方差矩阵

% 模拟一些观测数据
true_distance = 10; % 真实距离
measurements = true_distance + randn(1, 10) * sqrt(R); % 带有噪声的观测数据

% 卡尔曼滤波过程
for i = 1:length(measurements)
    % 预测步骤
    x_pred = A * x_est; % 预测下一个状态
    P_pred = A * P_est * A' + Q; % 预测误差协方差
    
    % 更新步骤
    K = P_pred * H' / (H * P_pred * H' + R); % 计算卡尔曼增益
    x_est = x_pred + K * (measurements(i) - H * x_pred); % 更新状态估计
    P_est = (eye(2) - K * H) * P_pred; % 更新误差协方差
end

% 显示结果
figure;
plot(measurements, 'b');
hold on;
plot(x_est(1, :), 'r--');
legend('Noisy Measurements', 'Kalman Filtered Estimates');
title('Kalman Filtered Distance Measurements');
xlabel('Time Step');
ylabel('Distance');